#ifndef _TRANSFORMEVALUATOR_
#define _TRANSFORMEVALUATOR_

#include <vector>
#include <iostream>
#include "BodyLink.h"
#include "Dof.h"
#include "Model.h"
#include "DofAbstract.h"

class Bodylink;
class Dof;
class Model;
class DofAbstract;

class TransformEvaluator{
protected:
	Model *mModel;
	double *mTimeStep;
	int mNumFrames;
	int mCurrFrame;	// keeps track of currently loaded frame
	int mOffset; // offset of the first frame b/t this and mDofs
	
	vector<vector<Mat4d*> *> mTrans; 
	vector<vector<vector<Mat4d*> > *> mFirstDeriv;
	vector<vector<vector<vector<Mat4d*> > > *> mSecondDeriv;

	vector<bool> *mActiveDofMap;	// active dofs map
public:
	void loadDofs(int _frame){
		if(_frame!=mCurrFrame){
			mCurrFrame=_frame;
			for(int i=0; i<mModel->getNumDofs(); i++){
				//if(mActiveDofMap->at(i)){	// BUG as even if the dof is inactive, correct value needs to be loaded
					mModel->getDof(i)->setValue(mDofs.at(i)->getValue(_frame));
				//}
			}
		}
	}

public:
	vector<DofAbstract*> mDofs;	// full state vector
	bool** mTransMap;
	bool*** mFirstDerivMap;
	bool**** mSecondDerivMap;

	TransformEvaluator(Model *_model, int _numFrames, double *_timestep, vector<bool> *_activeDofMap, int _offset);
	~TransformEvaluator();

	void resetNumFrames(int _frames);

	Mat4d getTrans(BodyLink *b, int frame);
	Mat4d getInvTrans(BodyLink *b, int frame);
	Mat4d getFirstDeriv(BodyLink *b, Dof *q, int frame);
	Mat4d getSecondDeriv(BodyLink *b, Dof *q1, Dof *q2, int frame);

	// full subtree computation
	void computeTrans(BodyLink *b, int frame);
	void computeFirstDeriv(BodyLink *b, int frame);
	void computeSecondDeriv(BodyLink *b, int frame);

	// cycle all the structures from start to end frames by 1
	void cycleStructures(int _frameStart, int _frameEnd);

	// invalidation functions
	inline void invalidateTrans(int _frame){
		memset(mTransMap[_frame], false, mModel->getNumBodyLinks()*sizeof(bool));
	}
	inline void invalidateFirstDeriv(int _frame){
		for(int i=0; i<mModel->getNumBodyLinks(); i++){
			memset(mFirstDerivMap[_frame][i], false, mDofs.size()*sizeof(bool));
		}
	}
	inline void invalidateSecondDeriv(int _frame){
		for(int i=0; i<mModel->getNumBodyLinks(); i++){
			for(int j=0; j<mDofs.size(); j++){
				memset(mSecondDerivMap[_frame][i][j], false, mDofs.size()*sizeof(bool));
			}
		}
	}
	inline void invalidateDofs(){
		mCurrFrame = -1;
	}
	inline void setCurrFrame(int _f){mCurrFrame = _f;}
	inline int getNumFrames(){return mNumFrames;}
	inline void setOffset(int _o){mOffset= _o;}

	Mat4d getTransDot(BodyLink *bi, int _frame);	// computes W dot
	Mat4d getTransDotPartialDof(BodyLink *bi, Dof *qk, int _frame);	// partial Wdot / partial qj
	Mat4d getTransDotPartialVel(BodyLink *bi, Dof *qk, int _frame);	// partial Wdot / partial qjdot
	//Mat4d getTransDotFirstDeriv(BodyLink *bi, int _frame, Dof *qk, int _dofFrame);	// computes dWdot/dqk .. qk can be at time t or t-1

	Mat4d getTransDotDot(BodyLink *bi, int _frame);	// computes W ddot
	Mat4d getTransDotDotApprox(BodyLink *bi, int _frame);	// backward finite diff of velocities

	// dofs derivatives
	inline double getVelToSlopeDeriv(){return 1/(*mTimeStep);}
	inline double getVelocity(int _dofid, int _frame){
		return mDofs[_dofid]->getSlope(_frame)*getVelToSlopeDeriv();
		//return (mDofs[_dofid]->getValue(_frame) - mDofs[_dofid]->getValue(_frame-1))/(*mTimeStep);
	}
	inline double getAccel(int _dofid, int _frame){
		return mDofs[_dofid]->getSecondSlope(_frame)*getVelToSlopeDeriv()*getVelToSlopeDeriv();
		//return (mDofs[_dofid]->getValue(_frame+1) - 2*mDofs[_dofid]->getValue(_frame) + mDofs[_dofid]->getValue(_frame-1))/((*mTimeStep)*(*mTimeStep));
	}

	inline Vec3d getCOM ( BodyLink* _b, int _frame ){ loadDofs(_frame); return _b->computeSubtreeCOM(); }
	inline vector<bool> *getActiveDofMap(){return mActiveDofMap;}

};

#endif
